/*!
  @file stereo_gc.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _KCV_STEREO_GC_H_
#define _KCV_STEREO_GC_H_

#include <boost/shared_ptr.hpp>
#include <opencv2/core/core.hpp>
#include "maxflow-v3.01/graph.h"

template class Graph<int, int, int>;

namespace kcv
{

/*! 
 *  @brief  Stereo Mathcing with Graph-cut 
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 */

class StereoGC 
{
 public:
  typedef Graph<int, int, long>        GraphType;
  typedef boost::shared_ptr<GraphType> GraphPtr;

  enum
  {
    DATA_COST_SAD,
    DATA_COST_ZNCC,
    DATA_COST_DIFF,
  };

  StereoGC(const int func_data=DATA_COST_SAD)
      : func_data_(func_data)
      , scale_(1.0)
      , min_disp_(0)
      , max_disp_(32)
      , win_size_(5)
      , debug_(false)
  { }

  ~StereoGC() { }

  void setDebug(bool debug) { debug_ = debug; }

  void setScale(const double scale) { scale_ = scale; }

  void setDisparityLimits(const int min_disp, const int max_disp)
  {
    min_disp_ = min_disp;
    max_disp_ = max_disp;
  }

  void setWindowSize(const int win_size)
  {
    win_size_ = win_size;
  }
  
  void process(const cv::Mat &imL, const cv::Mat &imR, cv::Mat &disparity);

  void show_image(const std::string &winname="image");
  void show_disparity(const std::string &winname="disparity");

 private:
  enum
  {
    LARGE_COST = 30,
  };

  void initImages(const cv::Mat &imL, const cv::Mat &imR);

  void performAlphaExtension();

  int calcDataCost(const int i, const int j, const int d);
  int sad(const int i, const int j, const int d);
  int zncc(const int i, const int j, const int d);
  int diff(const int i, const int j, const int d);

  int calcSmoothCost(const int u, const int v);


  inline int SITEIDX(const int i, const int j) { return i * imL_.cols + j; }

  template <typename T>
  inline int isBetween(const T &i, const T &lower, const T &upper)
  {
    return (i >= lower) && (i < upper);
  }

  int func_data_;
  double scale_;
  int min_disp_;
  int max_disp_;
  int win_size_;

  bool debug_;

  cv::Mat imL_;
  cv::Mat imR_;
  cv::Mat dis_;
};

} // namespace myns

#endif  // _KCV_STEREO_GC_H_

